#include "plock.hpp"
#include "rclcpp/rclcpp.hpp"
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

// #define TEST_TTY

#define uint8_to_uint16(hi, lo) ((uint16_t)(hi) << 8 | (uint16_t)(lo))
#define uint8_to_uint32(c0, c1, c2, c3)                                                        \
	((uint32_t)(c0) << 24 | (uint32_t)(c1) << 16 | (uint32_t)(c2) << 8 | (uint32_t)(c3))
#define uint8_to_int32(c0, c1, c2, c3)                                                         \
	((int32_t)((uint32_t)(c0) << 24) | (uint32_t)(c1) << 16 | (uint32_t)(c2) << 8 |            \
	 (uint32_t)(c3))
#define uint8_to_int16(hi, lo) ((int16_t)((int16_t)(hi)*256 + (uint16_t)(lo)))

// register 0x00 error code
namespace err_code
{
#define INTERNAL_COMMUNACTION 220 // internal communication error
#define HIGH_TEMPREATURE 252	  // device tempreature too high
#define LOW_TEMPREATURE 253		  // device tempreature too low
#define EXCEED_RANGE 254		  // exceed measure range(distance)
#define WEAK_REFLECTION 255		  // laser reflection signal too weak
#define STRONG_REFLECTION 256	  // laser reflection signal too strong
#define STRONG_AMBIENT 257		  // ambient light too strong

} // namespace err_code

/**
 * @brief convert frequency level to real frequency value(Hz), be wary, this returns 0 if
 * level is 0
 *
 * @param level 0, 1, 2, 3, 4
 * @return int 0, 5, 10, 20 or 30, -1: exceed range
 */
int level_to_Hz(int level);
/**
 * @brief senky laser modbus communication controller class
 *
 */
class Modbus_controller
{
  private:
	// linux file descriptor
	int fp;
	// remembered device address, and is used by all read and write actions, on first
	// communication, we can use 0 to send a broadcast read_address() to get a device's actual
	// address(1-247)
	uint8_t addr;
	// mutex with timeout, used to prevent nested call to methods and file descriptor from being
	// changed during communication, which may cause incomplete read/write
	std::timed_mutex tty_lock;

  public:
	// mutex lock shared by processes
	std::shared_ptr<plock> file_lock;

	/**
	 * @brief Construct a new Modbus_controller object and config tty port to 115200bps, 8 bit,
	 * no parity, 1 stop
	 *
	 * @param filep tty device file descriptor
	 * @param addr initial target device address(from 1 to 247, 0 for boardcast) used by all
	 * read_xxx() and write_xxx() methods, if you're not sure about the device's address, use 0
	 * for broadcast and call read_address() to get its address, then call set_address() to
	 * update this buffered value
	 */
	Modbus_controller(int filep, uint8_t addr = 0, std::string sem_name = "");
	/**
	 * @brief calculate crc checksum for given bytes
	 *
	 * @param dat data buffer
	 * @param len data length in bytes
	 * @return uint16_t crc checksum
	 */
	uint16_t get_crc(unsigned char *dat, int len);

	/**
	 * @brief read raw
	 * 
	 * @param datbuf data buffer
	 * @param buflen buffer length
	 * @return int 0: timeout, >0: length of received data, <0: error
	 */
	int read_raw(uint8_t *datbuf, int buflen);

	/**
	 * @brief read multiple words from register
	 *
	 * @param reg register address
	 * @param cnt how many words are expected(16 bits)
	 * @param datbuf output buffer
	 * @param buflen buffer length(in byte)
	 * @return int -1: error when read or write tty, -2: bad recv crc or length, -3: no enough
	 * buffer space -4: locked, >0: how many bytes were successfully read
	 */
	int read_regs(uint16_t reg, uint16_t cnt, uint8_t *datbuf, int buflen);

	/**
	 * @brief write one word to a register
	 *
	 * @param reg register address
	 * @param dat data to be writeen
	 * @return int 0: success, -1: error when read or write tty, -2: bad crc
	 */
	int write_reg(uint16_t reg, uint16_t dat);

	/**
	 * @brief read device error code register
	 *
	 * @return int 0: OK, <0: failed to read, >0: error code
	 */
	int read_error(void);

	/**
	 * @brief read status register
	 *
	 * @return int <0: failed, 0: idle, 1: laser pointing, 2: measuring
	 */
	int read_status(void);
	/**
	 * @brief read measure result(0.1mm)
	 *
	 * @return long <-2000: failed, 0: invalid data, >0: valid data
	 */
	int read_distance(void);

	/**
	 * @brief read measure offset value(0.1mm), measure - offset = real
	 *
	 * @return int 0x7FFFFFFF: failed, other: valid signed int
	 */
	int read_offset(void);

	/**
	 * @brief read device termpreature sensor data(0.1C)
	 *
	 * @return int 0x7FFFFFFF: failed, other: valid data
	 */
	int read_tempreature(void);

	/**
	 * @brief read firmware version(unsigned 16 bits)
	 *
	 * @return int <0: failed, other: 16bits data
	 */
	int read_version(void);

	/**
	 * @brief read sample frequency level
	 *
	 * @return int <0: failed, 0: single, 1: 5Hz, 2: 10Hz, 3: 20Hz, 4: 30Hz
	 */
	int read_frequency(void);

	/**
	 * @brief read device address register
	 *
	 * @return int <0: failed, 0: broadcast address, 1-247: valid address, other: invalid
	 */
	int read_address(void);

	/**
	 * @brief write status register
	 *
	 * @param status 0: idle, 1: laser pointing 2: measuring
	 * @return int <0: failed, 0: success
	 */
	int write_status(uint16_t status);

	/**
	 * @brief config measure frequency
	 *
	 * @param level 0: single, 1: 5Hz, 2: 10Hz, 3: 20Hz, 4: 30Hz
	 * @return int 0: success, <0: failed
	 */
	int write_frequency(uint16_t level);
	/**
	 * @brief write offset register
	 *
	 * @param offset 16 bit signed int(range from -20000 to 20000), in 0.1mm
	 * @return int <0: failed 0: success
	 */
	int write_offset(int16_t offset);

	/**
	 * @brief write device address register
	 *
	 * @param address range from 1 to 247, 0 for broaddcast
	 * @return int <0: failed, 0: success
	 */
	int write_address(uint16_t address);

	/**
	 * @brief change remembered address, NOTE: this will not write device address register,
	 * to do so, call write_address()
	 *
	 * @param address address to be set(from 1 to 127), 0 for broadcast
	 * @return int address set
	 */
	int set_address(uint16_t address = 0);

	/**
	 * @brief get remembered address, NOTE: this will not actually read device register, to do
	 * so, call read_address()
	 *
	 * @return int
	 */
	int get_address(void);

	/**
	 * @brief read unique product id
	 *
	 * @return long <0: faile, other: product id(low 32 bits)
	 */
	long read_id(void);
	/**
	 * @brief write all parameters into internal rom
	 *
	 * @return int 0: success, <0: failed
	 */
	int save_params(void);

	/**
	 * @brief tty loopback test
	 *
	 * @return int some error code
	 */
	int tty_test(void);
};
